#include "SolverUtil.h"
#include <malloc.h>
#include <stdio.h>
#include <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_eigen.h>

#define SINGULAR 1e-10

void assemblyBoundaryVector( SurfTrack* surf, double** b )
{
	int nTri = surf->m_mesh.num_triangles();

	vectorClear(b, nTri*3);

	for (int i=0; i<nTri; i++)
	{
		Vec3ui qtri = surf->m_mesh.get_triangle(i);

		Vec3d q0 = surf->get_position(qtri[0]);
		Vec3d q1 = surf->get_position(qtri[1]);
		Vec3d q2 = surf->get_position(qtri[2]);

		if ((q0[2] < 1e-5) && (q1[2] < 1e-5) && (q2[2] < 1e-5))
		{
			(*b)[3*i+2] = -0.5;
		}

		if ((q0[2] > (1 - 1e-5)) && (q1[2] > (1 - 1e-5)) && (q2[2] > (1 - 1e-5)))
		{
			(*b)[3*i+2] = 0.5;
		}
	}

	/*for (int i=0; i<3*nTri; i++)
	{
		(*b)[i] = i + 1;
	}*/
}

double vectorInnerProduct( double* a, double* b, int n )
{
	double s = 0;

	for (int i=0; i<n; i++)
	{
		s += a[i] * b[i];
	}

	return s;
}

int getIndexLS( int i, int j, int n )
{
	return i * n + j;
}

void vectorMinus( double* a, double* b, double** s, int n )
{
	for (int i=0; i<n; i++)
	{
		(*s)[i] = a[i] - b[i];
	}
}

void vectorCopy( double* src, double** trg, int n )
{
	for (int i=0; i<n; i++)
	{
		(*trg)[i] = src[i];
	}
}

void vectorAmplititude( double* src, double scalar, double** trg, int n )
{
	for (int i=0; i<n; i++)
	{
		(*trg)[i] = scalar * src[i];
	}
}

void xUpdate( double* src, double scalar1, double* vector1, double scalar2, double* vector2, double** trg, int n )
{
	for (int i=0; i<n; i++)
	{
		(*trg)[i] = src[i] + scalar1 * vector1[i] + scalar2 * vector2[i];
	}
}

void vectorClear( double** x, int n )
{
	for (int i=0; i<n; i++)
	{
		(*x)[i] = 0;
	}
}

void biCGStab( mvFun mvProduct, double* b, double** x, int n, int max_it, double tol )
{
	double *r, *r_tld, *p, *v, *s, *t;

	r = (double*)malloc(n * sizeof(double));
	r_tld = (double*)malloc(n * sizeof(double));
	p = (double*)malloc(n * sizeof(double));
	v = (double*)malloc(n * sizeof(double));
	s = (double*)malloc(n * sizeof(double));
	t = (double*)malloc(n * sizeof(double));

	double omega, alpha, rho, beta, rho_1, ns, bnrm2, error;

	omega = 1.0;
	alpha = 1.0;	

	bnrm2 = vectorInnerProduct(b, b, n);

	if (bnrm2 < SINGULAR)
	{
		bnrm2 = 1.0;
	}

	mvProduct(*x, &r, n);

	vectorMinus(b, r, &r, n);

	error = vectorInnerProduct(r, r, n) / bnrm2;

	if (error < tol)
	{
		return;
	}

	vectorCopy(r, &r_tld, n);

	for (int i=0; i<max_it; i++)
	{		
		printf("%d\n", i);

		rho = vectorInnerProduct(r_tld, r, n);

		if (abs(rho) < SINGULAR)
		{
			break;
		}

		if (i > 0)
		{
			beta = (rho / rho_1) * (alpha / omega);
			xUpdate(r, beta, p, -1 * beta * omega, v, &p, n);
		}else{
			vectorCopy(r, &p, n);
		}

		mvProduct(p, &v, n);

		alpha = rho / vectorInnerProduct(r_tld, v, n);

		vectorAmplititude(v, alpha, &s, n);
		vectorMinus(r, s, &s, n);

		ns = vectorInnerProduct(s, s, n);


		if (ns < tol)
		{
			vectorAmplititude(p, -1 * alpha, &p, n);
			vectorMinus(*x, p, x, n);
			break;
		}		

		mvProduct(s, &t, n);

		omega = vectorInnerProduct(t, s, n) / vectorInnerProduct(t, t, n);

		xUpdate(*x, alpha, p, omega, s, x, n);

		vectorAmplititude(t, omega, &r, n);

		vectorMinus(s, r, &r, n);

		error = vectorInnerProduct(r, r, n) / bnrm2;

		if (error < tol)
		{
			break;
		}

		if (abs(omega) < SINGULAR)
		{
			break;
		}

		rho_1 = rho;
	}

	free(r);
	free(r_tld);
	free(p);
	free(v);
	free(s);
	free(t);
}


void getEigenDecomposition( double* A, double** pval, double** pvec )
{
	gsl_matrix_view m = gsl_matrix_view_array(A, 3, 3);
	gsl_vector *eval = gsl_vector_alloc (3);
	gsl_matrix *evec = gsl_matrix_alloc (3, 3);
	gsl_eigen_symmv_workspace * w = gsl_eigen_symmv_alloc (3);

	gsl_eigen_symmv (&m.matrix, eval, evec, w);
	gsl_eigen_symmv_free (w);
	gsl_eigen_symmv_sort (eval, evec, GSL_EIGEN_SORT_VAL_ASC);

	for (int i=0; i<3; i++)
	{
		(*pval)[i] = gsl_vector_get(eval, i);
		gsl_vector col = gsl_matrix_column(evec, i).vector;

		(*pvec)[3*i+0] = gsl_vector_get(&col, 0);
		(*pvec)[3*i+1] = gsl_vector_get(&col, 1);
		(*pvec)[3*i+2] = gsl_vector_get(&col, 2);
	}



	gsl_vector_free (eval);
	gsl_matrix_free (evec);
}
